#include "Pathing.h"
#include <list>
#include <float.h>
#include <stack>

namespace Pathing
{
	bool PathObstacleTest( const PathingPacket& path, const float vehicleRadius, Obstacle obstacle, Maths::Vector3D& closestPoint, float& distance, Maths::Vector3D& obstacleToPath )
	{
		const float totalRadius = vehicleRadius + obstacle.Radius();
		const Maths::Vector3D relPosition = obstacle.Position() - path.start;

		const Maths::Vector3D direction = (path.end - path.start).Normal();

		distance = relPosition.Dot(direction);
		closestPoint = (direction * distance);
		obstacleToPath = closestPoint - relPosition;
		closestPoint += path.start;

		return (obstacleToPath.Magnitude() < totalRadius && distance > 0);
	}
	
	State PathObstaclesTest( const PathingPacket& path, const float vehicleRadius, const Obstacle::Vector_t& obstacles, Obstacle*& closestObstacle, Maths::Vector3D& closestPoint, float& distance, Maths::Vector3D& obstacleToPath )
	{
		State pathingState = NO_INTERSECTION;
		distance = FLT_MAX;

		float tempDistance;
		Maths::Vector3D tempObstacleToPath;
		Maths::Vector3D tempClosestPoint;

		for (Obstacle::Vector_t::const_iterator it = obstacles.begin(); it != obstacles.end(); ++it)
		{
			if (PathObstacleTest(path, vehicleRadius, **it, tempClosestPoint, tempDistance, tempObstacleToPath))
			{
				if (tempDistance == distance)					// Simultaneous intersection?
				{
					pathingState = SIMULTANEOUS_INTERSECTION;
				}
				else if (tempDistance < distance)					// Closest intersection?
				{
					distance = tempDistance;
					obstacleToPath = tempObstacleToPath;
					closestObstacle = *it;
					closestPoint = tempClosestPoint;
					pathingState = SINGLE_INTERSECTION;
				}
			}
		}
		return pathingState;
	}
//		Maths::DtoP.Normal() * (DtoP.Magnitude() - totalRadius);
	bool GetPathofVehicleToDestination( Vehicle& vehicle, const Obstacle::Vector_t& obstacles, float& costOfPath, std::list<Maths::Vector3D>& path, const unsigned int maxDepth, const unsigned int depth )
	{
		PathingPacket pathingPacket;
		pathingPacket(vehicle.Position(), vehicle.Destination());

		bool success = TestPath(pathingPacket, vehicle.Radius(), obstacles, costOfPath, path, maxDepth, depth + 1);
		
		if (success)
			vehicle.PopPathingPoint();

		return success;
	}
	// Recursive function

	bool TestPath( const PathingPacket& path, const float vehicleRadius, const Obstacle::Vector_t& obstacles, float& costOfPath, std::list<Maths::Vector3D>& pathSoFar, const unsigned int maxDepth, const unsigned int depth )
	{	
		if (depth > maxDepth)
			return false;

		float distance;
		Maths::Vector3D obstacleToPath;
		Maths::Vector3D closestPoint;
		Obstacle* closestObstacle = 0;

		State pathingState = PathObstaclesTest( path, vehicleRadius, obstacles, closestObstacle, closestPoint, distance, obstacleToPath);

		switch (pathingState)
		{
		case NO_INTERSECTION:
			{
				pathSoFar.push_back(path.end);					// Just add this destination
				costOfPath = path.Length();
				return true;
			};
		case SIMULTANEOUS_INTERSECTION:
			{
				return false;							// Failed. Do not progress down path that is unwalkable
			};
		case SINGLE_INTERSECTION:				// Find possible path of least cost
			{
				bool option1success = false, option2success = false;				
				std::list<Maths::Vector3D> path1Nodes;
				std::list<Maths::Vector3D> path2Nodes;

				float totalCostOfPath1;
				float totalCostOfPath2;
				float costOfPath1[2];
				float costOfPath2[2];

				PathingPacket path1[2];
				PathingPacket path2[2];

				GetAdjustedPaths(path, vehicleRadius,*closestObstacle, obstacleToPath, closestPoint, path1, path2);

				if (TestPath(path1[0], vehicleRadius, obstacles, costOfPath1[0], path1Nodes, maxDepth, depth + 1) && TestPath(path1[1], vehicleRadius, obstacles, costOfPath1[1], path1Nodes, maxDepth, depth + 1))
				{
					totalCostOfPath1 = costOfPath1[0] + costOfPath1[1];
					option1success = true;
				}
				if (TestPath(path2[0], vehicleRadius, obstacles, costOfPath2[0], path2Nodes, maxDepth, depth + 1) && TestPath(path2[1], vehicleRadius, obstacles, costOfPath2[1], path2Nodes, maxDepth, depth + 1))
				{
					totalCostOfPath2 = costOfPath2[0] + costOfPath2[1];
					option2success = true;
				}

				if (option1success && option2success)
				{
					if (totalCostOfPath1 < totalCostOfPath2)
					{
						pathSoFar.insert(pathSoFar.end(), path1Nodes.begin(), path1Nodes.end());
						costOfPath = totalCostOfPath1;
						return true;
					}
					else
					{
						pathSoFar.insert(pathSoFar.end(), path2Nodes.begin(), path2Nodes.end());
						costOfPath = totalCostOfPath2;
						return true;
					}
				}
				else if (option1success)
				{
					pathSoFar.insert(pathSoFar.end(), path1Nodes.begin(), path1Nodes.end());
					costOfPath = totalCostOfPath1;
					return true;
				}
				else if (option2success)
				{
					pathSoFar.insert(pathSoFar.end(), path2Nodes.begin(), path2Nodes.end());
					costOfPath = totalCostOfPath2;
					return true;
				} else
				{
					return false;
				}
			};
		}
		return false;
	}

	// FindPath searches for the most efficient path from a point to a point
	// while moving between and around obstacles.
	// Returns false if no path is found.

	void GetAdjustedPaths( const PathingPacket& path, const float vehicleRadius, const Obstacle& closestObstacle, const Maths::Vector3D& obstacleToPath, const Maths::Vector3D& closestPoint, PathingPacket* path1, PathingPacket* path2 )
	{
		// Find the 2 destinations to get around closest obstacle
		const float obstacleToPathLength = obstacleToPath.Magnitude();
		const float extRadius = vehicleRadius + closestObstacle.Radius();
		Maths::Vector3D perp(path.end - path.start);
		const Maths::Vector3D obstacleToPathNorm = obstacleToPathLength > 0 ? obstacleToPath.Normal() : (Maths::Vector3D(perp.Y(), perp.X(), perp.Z())).Normal();
		const float offset1 = extRadius - obstacleToPathLength;
		const float offset2 = offset1 - ( extRadius * 2);
		const Maths::Vector3D destination1 = (obstacleToPathNorm * (offset1 * 1.5f)) + closestPoint;
		const Maths::Vector3D destination2 = (obstacleToPathNorm * (offset2 * 1.5f)) + closestPoint;

		// Vehicles to get to each path
		path1[0](path.start, destination1);
		path2[0](path.start, destination2);

		// Vehicles to get from the end of that path to out current destination
		path1[1](destination1, path.end);
		path2[1](destination2, path.end);
	}

	bool LOSVehicleNextNode( Vehicle& vehicle, const Obstacle::Vector_t& obstacles )
	{
		PathingPacket pathing;
		Obstacle* obstacle;
		Maths::Vector3D closestPoint;
		float distance;
		Maths::Vector3D obstacleToPath; 

		pathing(vehicle.Position(), vehicle.PeekPathingPoint());

		if (PathObstaclesTest( pathing, vehicle.Radius(), obstacles, obstacle, closestPoint, distance, obstacleToPath) == NO_INTERSECTION)
		{
			return true;
		}
		return false;
	}

};
